// pid.c（单级pid）
#include "pid.h"


//变量定义
static float Kp, Ki, Kd;
static float err, last_err, last_last_err;
static float output=200; //初始值

//pid参数初始化
void pid_init(float p, float i, float d) {

    Kp = p; Ki = i; Kd = d;
   
    //初始化误差值
    err = last_err = last_last_err = 0;

    // integral = 0;
}

//pid角度控制函数
float pid_calculate(float pitch_angle) {

    err = 0 - pitch_angle;//俯仰角目标值为0
    
    
   // integral += err;
    float derivative = err - last_err;// 计算一阶导数
    float second_derivative = err - 2*last_err + last_last_err;// 计算二阶导数

    float output0 = Kp * derivative + Ki * err + Kd * second_derivative;
    


    output += output0; 

    last_last_err = last_err;
    last_err = err;

    if (output >= 240) output = 240;
    if (output <= 160) output = 160;//限制输出范围


    return output;//
}
